
/***************************************************************************** \
 *                              ColWrapperOpenSceneGraph
\*****************************************************************************/

/*! @file
 *
 *  @brief
 *    Wrapper functions, which convert the OpenSceneGraph objects into the
 *    corresponding CollDet objects.
 *
 *  Normally these wrapper functions will be used as callback
 *  function, pass to the collision pipeline as a parameter
 *
 *  see makeCollidable
 *
 *  @author Weiyu Yi, wyi@tu-clausthal.de
 *
 */

//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <stdio.h>

#include <osg/Math>
#include <osg/Geometry>
#include <osg/PrimitiveSet>

#define COL_EXPORT

#include <col_import_export.h>
#include <ColWrapperOpenSceneGraph.h>

/// Collision detection namespace
namespace col {

/** callback function which convert the openscenegraph geometry into
 *  corresponding colldet geometry.
 *
 * @param ColGeometry*  pointer to a tobe assigned colldet geometry
 * @param void*   pointer to the opensg geometry object
 *
 * the openscenegraph geometry's pointer has to be changed into void*
 * type for the consistence of the pipeline interface.
 *
 */
bool convertGeometryFromOpenSceneGraph( ColGeometry * geo, void * geo_source )
{
	// assert
	
	// clear the old data in geometry
	geo->clearOldData();
	
	const osg::ref_ptr<const osg::Geometry> geo_osg = static_cast<const osg::Geometry *>( geo_source );

	// for vertices
	const osg::ref_ptr<const osg::Vec3Array> vertices = dynamic_cast<const osg::Vec3Array *>( geo_osg->getVertexArray() );
	for ( unsigned int i=0; i<vertices->size(); i++ )
	{
		geo->editPointsPtr()->push_back( Point3( vertices->at( i )[0], vertices->at( i )[1], vertices->at( i )[2] ) );
	}

	// for primitives
	for ( unsigned int i=0; i<geo_osg->getNumPrimitiveSets(); i++ )
	{
		Primitive prim;
		const osg::ref_ptr<const osg::PrimitiveSet> ps = geo_osg->getPrimitiveSet( i );
		for ( unsigned int j=0; j<ps->getNumIndices(); j++ )
		{
			prim.push_back( ps->index( j ) );
		}
		geo->editPrimitivesPtr()->push_back( prim );
	}
	
	// update bbox
	geo->updateBBox();

	return true;
}

/** callback function which convert the openscenegraph matrix into colldet
 *	matrix.
 *
 * @param Matrix4*  pointer to the colldet matrix
 * @param void*  pointer to the opensg matrix
 *
 * see convertGeometryFromOpenSceneGraph
 */
bool convertMatrixFromOpenSceneGraph( Matrix4 * tran, void * tran_source )
{
	const osg::Matrixf *source = static_cast<const osg::Matrixf *>(tran_source);

	Vector4 vec0( (*source)(0, 0),(*source)(0, 1), (*source)(0, 2), (*source)(0, 3) );
	Vector4 vec1( (*source)(1, 0),(*source)(1, 1), (*source)(1, 2), (*source)(1, 3) );
	Vector4 vec2( (*source)(2, 0),(*source)(2, 1), (*source)(2, 2), (*source)(2, 3) );
	Vector4 vec3( (*source)(3, 0),(*source)(3, 1), (*source)(3, 2), (*source)(3, 3) );

	// tran has be allocated but not be initialized
	// so all vertors in matrix must be assigned.
	tran->setCol( 0, vec0 );
	tran->setCol( 1, vec1 );
	tran->setCol( 2, vec2 );
	tran->setCol( 3, vec3 );

	return true;
}

} //namespace col
